#pragma once
#include <stdio.h>
#include <intrin.h>
#include <smmintrin.h>

#include<string>
#include<map>
#include<vector>

#include "Shape/PointCloud.h"
#include "Geometry/Vector3.h"
#include "Geometry/BoundingBox.h"
#include "System/Platform.h"

#include<Eigen/Dense>
#include <pcl/kdtree/kdtree_flann.h>
#include <boost/interprocess/file_mapping.hpp>
#include <boost/interprocess/mapped_region.hpp>

namespace PCS
{
	namespace Core
	{     	

		struct statOOC {
				int threadId;
				float elapsed;
		};

		struct cellElementOOC {
				grid_key key;
				std::vector<size_t>* points;
		};

		struct cell_data {
				bool in_memory;
				size_t count;
				size_t offset;
		};

		class SparseGridOutOfCore
		{
		protected:
			Vector3 m_size;											//number of cells along the three axis of the grid.
			Vector3 m_resolution;									//resolution along the X,Y,Z axis of the grid
			Vector3 m_extent;
			AxisAlignedBoundingBox * m_pBounds;						//bounding volume for the point cloud assigned to this space ... not necessarily the same as the point cloud volume.
			std::map<grid_key, std::vector<size_t>*> * m_pGrid;		//map data structure used to store points in cloud
			std::map<grid_key, cell_data> * m_pGridOOC;
			PointCloud * m_pointCloud;
			float m_buffer;											//expand this sparse grid bounding volume by this buffer 
			float m_neighbourRadiusSq;
			
			float m_extent_x, m_extent_y, m_extent_z;
			size_t totalPoints;

			int averageBinSize;
			int minBinSize;
			int maxBinSize;

			float r_average;

			size_t * r_addr;

		public:			
			SparseGridOutOfCore(void) { m_resolution = 10.0f; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGridOutOfCore(float p_resolution) { m_resolution = p_resolution; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGridOutOfCore(Vector3 * p_resolution) { m_resolution = *p_resolution; m_buffer = 0.1f; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }
			SparseGridOutOfCore(float p_resolution, float p_buffer) { m_resolution = p_resolution; m_buffer = p_buffer; m_pGrid = new std::map<grid_key, std::vector<size_t>*>(); }

			~SparseGridOutOfCore() 
			{
				delete m_pGrid;
				delete m_pointCloud;
			}

			inline static unsigned int gridHash(int x, int y, int z) {
				return ((x & 0x3FF) << 20) | ((y & 0x3FF) << 10) | (z & 0x3FF);
			}

			AxisAlignedBoundingBox * GetBounds() {
				return m_pBounds;
			}

			int GetAverageBinSize() {
				return averageBinSize;
			}

			int GetMinBinSize() {
				return minBinSize;
			}

			int GetMaxBinSize() {
				return maxBinSize;
			}

			Vector3 GetSize() {
				return m_size;
			}

			Vector3 GetResolution() {
				return m_resolution;
			}

			Vector3 GetExtent() {
				return m_extent;
			}

			int getTotalPoints() {
				return totalPoints;
			}

			std::map<grid_key, std::vector<size_t>*> * GetMap() {
				return m_pGrid;
			}

			PCS::Core::PointCloud * GetPointCloud() {
				return m_pointCloud;
			}			

			void SetBoundsWithBuffer(AxisAlignedBoundingBox * p_bounds) 
			{ 
				Vector3 gridBuffer = p_bounds->GetExtent() * 2 * m_buffer;
				m_pBounds = new AxisAlignedBoundingBox(p_bounds->GetMinExtent()-gridBuffer, p_bounds->GetMaxExtent()+gridBuffer); 
			} 

			void SetBoundsNoBuffer(AxisAlignedBoundingBox * p_bounds) 
			{ 
				m_pBounds = new AxisAlignedBoundingBox(p_bounds->GetMinExtent(), p_bounds->GetMaxExtent()); 
			} 								

			/**Generate the sparse grid structure and insert all point within. Then update point type.*/
			int Initialise(PointCloud* &p_cloud, float p_histogramDensity) 
			{				
				boost::timer timer;				

				m_pointCloud = p_cloud;

				std::map<grid_key, std::vector<size_t>*> pGrid = std::map<grid_key, std::vector<size_t>*>();

				SetBoundsWithBuffer((AxisAlignedBoundingBox*)(p_cloud->GetBoundingVolume()));
				m_extent.X = (m_pBounds->GetMaxExtent().X) - (m_pBounds->GetMinExtent().X); 
				m_extent.Y = (m_pBounds->GetMaxExtent().Y) - (m_pBounds->GetMinExtent().Y);
				m_extent.Z = (m_pBounds->GetMaxExtent().Z) - (m_pBounds->GetMinExtent().Z);	
				m_extent.W = 0.0f;

				//Now determine number of cells across the X, Y and Z dimensions. Static for now.
				m_size.X = m_extent.X / m_resolution.X;
				m_size.Y = m_extent.Y / m_resolution.Y;
				m_size.Z = m_extent.Z / m_resolution.Z;
				m_size.W = 0.0f;
				std::cout << "Number of cells along X, Y and Z = " << m_size.ToString() << std::endl;

				//Set neighbourdistance as the average of the resolutions and divide by 2 ... heuristic.
				m_neighbourRadiusSq = ((m_resolution.X + m_resolution.Y + m_resolution.Z) / 2) * ((m_resolution.X + m_resolution.Y + m_resolution.Z) / 2);				

				//Now iterate over the points in the cloud and insert them in their respective sgrid cells							
				std::vector<size_t>* v;
				std::map<grid_key, std::vector<size_t>*>::iterator gridIterator;

				int xIdx, yIdx, zIdx;				
				int xIdxOld, yIdxOld, zIdxOld;				
				int bins = 0;			
				r_average = 0.0f;
				averageBinSize = 0;
				minBinSize = 100;
				maxBinSize = 0;
				xIdxOld = yIdxOld = zIdxOld = -1000;
				__m128 a,b,c,d;

				b = _mm_load_ps(m_pBounds->GetMinExtent().Element);
				c = _mm_load_ps(m_size.Element);
				d = _mm_load_ps(m_extent.Element);																				
													
				size_t idx = 0;

				for (std::vector<Point*>::iterator pointIterator = p_cloud->GetPoints()->begin(); 
					 pointIterator != p_cloud->GetPoints()->end(); ++pointIterator, ++idx)
				{		
					//Implement the following using SSE intrinsics
					//xIdx = (int)floor((float)((((*pointIterator)->position.X - m_pBounds->GetMinExtent().X) * m_size.X) / m_extent.X));
					//yIdx = (int)floor((float)((((*pointIterator)->position.Y - m_pBounds->GetMinExtent().Y) * m_size.Y) / m_extent.Y));
					//zIdx = (int)floor((float)((((*pointIterator)->position.Z - m_pBounds->GetMinExtent().Z) * m_size.Z) / m_extent.Z));									

					a = _mm_load_ps((*pointIterator)->position.Element);																	
					a = _mm_sub_ps(a, b);
					a = _mm_mul_ps(a, c);
					a = _mm_div_ps(a, d);
					a = _mm_floor_ps(a);
					
					xIdx = a.m128_f32[0];
					yIdx = a.m128_f32[1];
					zIdx = a.m128_f32[2];					
					
					//If indices have not changed, insert in the same grid cell.
					if (xIdx==xIdxOld && yIdx == yIdxOld && zIdx == zIdxOld) {
						gridIterator->second->push_back(idx);
						if (gridIterator->second->size() > maxBinSize) maxBinSize = gridIterator->second->size();
						if (gridIterator->second->size() < minBinSize) minBinSize = gridIterator->second->size();
						//std::cout << "Adding to previous cell ... #" << gridIterator->second->size() << std::endl; 
						continue;
					}

					//Insert point in grid at the computed key.					
					if ((gridIterator = pGrid.find(gridHash(xIdx, yIdx, zIdx))) == pGrid.end())
					{
						//Create a new vector, insert this point and include in the hash map.																								
						v = new std::vector<size_t>();
						v->push_back(idx);
						(pGrid)[gridHash(xIdx, yIdx, zIdx)] = v;
						gridIterator = pGrid.find(gridHash(xIdx, yIdx, zIdx));
						bins++;
						//std::cout << "Creating new cell ..." << xIdx << "," << yIdx << "," << zIdx << " -> " << gridHash(xIdx, yIdx, zIdx) << std::endl; 
					}
					else
					{
						//Retreive the vector attached to this grid cell and add the new point to it.
						gridIterator->second->push_back(idx);
						if (gridIterator->second->size() > maxBinSize) maxBinSize = gridIterator->second->size();
						if (gridIterator->second->size() < minBinSize) minBinSize = gridIterator->second->size();
						//std::cout << "Adding to cell ..." << xIdx << "," << yIdx << "," << zIdx << " -> " << gridHash(xIdx, yIdx, zIdx) << " #" << gridIterator->second->size() <<std::endl;
					}
					
					xIdxOld = xIdx;
					yIdxOld = yIdx;
					zIdxOld = zIdx;
				}

				totalPoints = p_cloud->GetSize();				
				averageBinSize = totalPoints / bins;
											
				std::cout << boost::str(boost::format("[SparseGridOOC] Populated in %ds. Bin# = %d, avg = %d max = %d min = %d.") % timer.elapsed() % bins % averageBinSize % maxBinSize % minBinSize) << std::endl;
							

				//if (p_histogramDensity != 0) PrintDensityHistogram(p_histogramDensity);
				//UpdatePointType_DD();				
				 
				return 0;
			};



			void PrintDensityHistogram(float p_binDensity) {
				float binDensitySpan = GetMaxBinSize() - GetMinBinSize();
				float binWidth = binDensitySpan * p_binDensity;
				int numberOfBins = (int) (binDensitySpan / binWidth);
				std::vector<int> bins = std::vector<int>();
				for (int i = 0; i<numberOfBins+1; i++) bins.push_back(0);
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator) {					

					bins[(int)(cellIterator->second->size()/binWidth)]+=1;
				}
				std::cout << "[SparseGridOOC] Histogram ******" << std::endl; 
				for (int i = 0; i<numberOfBins+1; i++) {
					if (bins[i] != 0)
						std::cout << "[SparseGrid] Bin " << i << " [" << i*binWidth << "-" << (i*binWidth)+binWidth << "] = " << bins[i] << std::endl;
				}
				std::cout << "[SparseGridOOC] Histogram ******" << std::endl; 
			}
			
			/**Iterate over the points in each cell of the grid, setting their type (surface or edge)*/
			void UpdatePointType_DD() {
				stat * stats = new stat[4];
				cellElement * cellsInGrid = new cellElement[m_pGrid->size()];
				boost::timer timer;
				int neighbourCount = 0;
				int pointsChecked = 0;
				int notEnoughNeighbours = 0;
				
				int edgePoints = 0;
				int surfacePoints = 0;
				float eigenRatioEdge = 0.0f;

				int numberOfProcs = omp_get_num_procs();				
				//omp_set_num_threads(16);
				//numberOfProcs = 16;
				std::cout << "[SparseGridOOC] " << numberOfProcs << " threads starting to set point types"  << std::endl;
								
				//Initialise shared array of points to the cells in the sgrid				
				int idx = 0;
				for (std::map<grid_key, std::vector<size_t>*>::iterator cellIterator = m_pGrid->begin(); cellIterator != m_pGrid->end(); ++cellIterator)				
				{
					cellsInGrid[idx].key = cellIterator->first;
					cellsInGrid[idx].points = cellIterator->second;
					idx+=1;
				}
								
				int size = idx;
				idx = 0;

				//Initialise Memeory Mapped File which will temporarily store neighbourhood query results.				
				//boost::interprocess::file_mapping::remove(FileNameQBuffer);
				int n_space = N_FOR_CRAWL+1;
				std::filebuf fbuf;
				fbuf.open(FileNameQBuffer, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);
				fbuf.pubseekoff((totalPoints*n_space*sizeof(size_t))-sizeof(char), std::ios_base::beg);				
				fbuf.sputc(0);
				fbuf.close();

				boost::interprocess::file_mapping m_file(FileNameQBuffer, boost::interprocess::read_write);
				boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

				r_addr = (size_t*)region.get_address();				
				size_t r_size = region.get_size();				
				std::cout << "[SparseGridOOC] MMF created and mapped to region. Size= " << r_size << std::endl;
				
				#pragma omp parallel for schedule(guided)
				for (int i=0; i<numberOfProcs; i++) {
					float elapsedStart = timer.elapsed();
					int localNeighbourCount = 0;
					int localPointsChecked = 0;
					int localNotEnoughNeighbours = 0;
					int xIdxP; int yIdxP; int zIdxP;
					int localEdgePoints = 0;
					int localSurfacePoints = 0;
					int increaseNeighbours = 0;
					int increaseInNeighbourhoodQueries = 0;
					bool incDone = false;
					int breakIncreaseNeighbours = 0;
					int pointsInBlock = 0;

					float minDistance = 100000;
					float maxDistance = 0;
					float crtDistance = 0;
					float medianDistance = 0;

					Eigen::Matrix3f covariance = Eigen::Matrix3f();				
					Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
					
					std::vector<std::vector<size_t>*> neighbourCells = std::vector<std::vector<size_t>*>();
					std::map<grid_key, std::vector<size_t>*> neighbourCellsMap = std::map<grid_key, std::vector<size_t>*>();
					std::vector<size_t> neighbours = std::vector<size_t>();	
					std::vector<size_t> pointList = std::vector<size_t>();
					grid_key cellKey;
					std::map<grid_key, std::vector<size_t>*>::iterator gridIterator;
					//std::vector<grid_key> nextPass = std::vector<std::string>();					
					
					int nextCell = 0;
					while (true) 
					{										
						//Determine which adjacent cells need to be considered and pass them to the neighbourhood query
						neighbourCells.clear();						
						nextCell = _InterlockedExchangeAdd(reinterpret_cast<volatile long*>(&idx), 1);
						if (nextCell > size-1) 
							break;
						cellKey = cellsInGrid[nextCell].key;
						gridIterator = m_pGrid->find(cellKey);
						
						//first search the voxel this point is located in						
						neighbourCells.push_back((*gridIterator).second);
						pointsInBlock = 0; //reset number of points in this block
						//then its adjacent ones
						unsigned int nck_x, nck_y;
						for (int xIdx = -1; xIdx < 2; ++xIdx) {
							nck_x = cellKey + (xIdx << 20);
							for (int yIdx = -1; yIdx < 2; ++yIdx) {
								nck_y = nck_x + (yIdx << 10);
								for (int zIdx = -1; zIdx < 2; ++zIdx) {
									if (!(xIdx==0 && yIdx==0 && zIdx==0)) {										
										if ((gridIterator = m_pGrid->find(nck_y+zIdx)) != m_pGrid->end())	{
											neighbourCells.push_back((*gridIterator).second);
											pointsInBlock+=(*gridIterator).second->size();											
										}
									}
								}
							}
						}
						
						if (pointsInBlock > KDTREE_THOLD) {
							//create KDTREE on the local set of points and use it for neighbourhood queries							
							pcl::PointCloud<pcl::PointXYZ>::Ptr pcd_cloud (new pcl::PointCloud<pcl::PointXYZ>);
							pointList.clear();
							for (std::vector<std::vector<size_t>*>::iterator cellIterator = neighbourCells.begin(); cellIterator != neighbourCells.end(); ++cellIterator) {
								for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) { 
									pcd_cloud->push_back(pcl::PointXYZ(m_pointCloud->GetPointAt(*pointIterator)->position.X, m_pointCloud->GetPointAt(*pointIterator)->position.Y, m_pointCloud->GetPointAt(*pointIterator)->position.Z)); 
									pointList.push_back(*pointIterator);
								}
							}
							pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
							kdtree.setInputCloud(pcd_cloud);
							std::vector<int> pointIdxNKNSearch(N_FOR_TYPE);
							std::vector<float> pointNKNSquaredDistance(N_FOR_TYPE);
							int idx=0;
							size_t crtIdx;
							for (std::vector<size_t>::iterator pointIterator = cellsInGrid[nextCell].points->begin();
								pointIterator != cellsInGrid[nextCell].points->end(); ++ pointIterator, ++ idx) {
									kdtree.nearestKSearch (idx, N_FOR_TYPE, pointIdxNKNSearch, pointNKNSquaredDistance);
									neighbours.clear();
									for (int i = 0; i<N_FOR_TYPE; i++) neighbours.push_back(pointList.at(pointIdxNKNSearch[i]));									
									//update MMF with neighbours data	
									crtIdx = *pointIterator;
									//std::cout << "Setting memory at base location " << crtIdx*n_space <<std::endl;
									neighbours.size() < N_FOR_CRAWL ? r_addr[crtIdx*n_space] = neighbours.size() :
																	  r_addr[crtIdx*n_space] = N_FOR_CRAWL;
									for (int nn = 1; nn<=neighbours.size() && nn<=N_FOR_CRAWL; nn++) { 
										//std::cout << "T" << omp_get_thread_num() << " Setting memory at offset location " << crtIdx*n_space+nn << " with nn=" << nn << " and npoint idx=" << pointList.at(pointIdxNKNSearch[nn]) <<std::endl;
										r_addr[crtIdx*n_space+nn] = pointList.at(pointIdxNKNSearch[nn]);
									}
									pointIdxNKNSearch.clear();
									pointNKNSquaredDistance.clear();
									if (neighbours.size() < N_FOR_TYPE) localNotEnoughNeighbours++;
									else {
										crtDistance = Vector3::DistanceSquared(m_pointCloud->GetPointAt(neighbours[N_FOR_TYPE-1])->position, m_pointCloud->GetPointAt(*pointIterator)->position);
										if (crtDistance > maxDistance) maxDistance = crtDistance;
										if (crtDistance < minDistance) minDistance = crtDistance;
										CalcCovMatrix(neighbours, N_FOR_TYPE, covariance);
										eigensolver.compute(covariance);
										if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) {											
												m_pointCloud->GetPointAt(*pointIterator)->T = edge;
												localEdgePoints++;											
										} else {
													//std::cout << "S- " << eigensolver.eigenvalues()[0] << "(" << eigensolver.eigenvalues()[0] * CURVATURE << ") - " << eigensolver.eigenvalues()[1] << eigensolver.eigenvalues()[2] << std::endl;
													m_pointCloud->GetPointAt(*pointIterator)->T = surface;
													localSurfacePoints++;													
												}
									}
									localPointsChecked++;
									if (localPointsChecked % 250000 == 0) 
										std::cout << boost::str(boost::format("[SparseGrid] T%d - #=%d") % omp_get_thread_num() % localPointsChecked) << " -> " << timer.elapsed() << std::endl;
							}							
							continue;
						}

						gridIterator = m_pGrid->find(cellKey);
						size_t crtIdx;
						for (std::vector<size_t>::iterator pointIterator = cellsInGrid[nextCell].points->begin();
							pointIterator != cellsInGrid[nextCell].points->end(); ++ pointIterator) {
								GetNeighbours(*pointIterator, neighbourCells, neighbours, N_FOR_TYPE);
								//update MMF with neighbours data	
								crtIdx = *pointIterator;
								//std::cout << "Setting memory at base location " << crtIdx*n_space <<std::endl;
								neighbours.size() < N_FOR_CRAWL ? r_addr[crtIdx*n_space] = neighbours.size() :
																  r_addr[crtIdx*n_space] = N_FOR_CRAWL;
								for (int nn = 1; nn<=neighbours.size() && nn<=N_FOR_CRAWL; nn++) { 
									//std::cout << "T" << omp_get_thread_num() << " Setting memory at offset location " << crtIdx*n_space+nn << " with nn=" << nn << " and npoint idx=" << pointList.at(pointIdxNKNSearch[nn]) <<std::endl;
									r_addr[crtIdx*n_space+nn] = neighbours[nn-1];
								}
								if (neighbours.size() < N_FOR_TYPE) localNotEnoughNeighbours++;
								else {
										crtDistance = Vector3::DistanceSquared(m_pointCloud->GetPointAt(neighbours[N_FOR_TYPE-1])->position, m_pointCloud->GetPointAt(*pointIterator)->position);
										if (crtDistance > maxDistance) maxDistance = crtDistance;
										if (crtDistance < minDistance) minDistance = crtDistance;
										CalcCovMatrix(neighbours, N_FOR_TYPE, covariance);
										eigensolver.compute(covariance);
										if (eigensolver.eigenvalues()[0] * CURVATURE > eigensolver.eigenvalues()[1]) {											
												m_pointCloud->GetPointAt(*pointIterator)->T = edge;
												localEdgePoints++;											
										} else {
													//std::cout << "S- " << eigensolver.eigenvalues()[0] << "(" << eigensolver.eigenvalues()[0] * CURVATURE << ") - " << eigensolver.eigenvalues()[1] << eigensolver.eigenvalues()[2] << std::endl;
													m_pointCloud->GetPointAt(*pointIterator)->T = surface;
													localSurfacePoints++;													
												}
								}
								localPointsChecked++;
								if (localPointsChecked % 250000 == 0) 
									std::cout << boost::str(boost::format("[SparseGrid] T%d - #=%d") % omp_get_thread_num() % localPointsChecked) << " -> " << timer.elapsed() << std::endl;
						}						
					}

					std::cout << boost::str(boost::format("[SparseGrid] T%d (%ds) #=%d NEN=%d MaxD=%d MinD=%d IncN=%d IncNQ=%d") % omp_get_thread_num() % timer.elapsed() % localPointsChecked % localNotEnoughNeighbours  % sqrt(maxDistance) % sqrt(minDistance) % increaseNeighbours % increaseInNeighbourhoodQueries) << std::endl;					
					stats[omp_get_thread_num()].threadId = omp_get_thread_num();
					stats[omp_get_thread_num()].elapsed = timer.elapsed() - elapsedStart;
					//write totals to shared variables
					#pragma omp critical(inc_patchid)
					{
						pointsChecked += localPointsChecked;
						edgePoints += localEdgePoints;
						surfacePoints += localSurfacePoints;
						notEnoughNeighbours += localNotEnoughNeighbours;
					}

				}  //end pragma									
				region.flush();
				std::cout << "[SparseGrid] All threads finished updating point types in " << timer.elapsed() << std::endl;				
				std::cout << boost::str(boost::format("[SparseGrid] Totals #=%d E=%d S=%d NEN=%d") % pointsChecked % edgePoints % surfacePoints % notEnoughNeighbours) << std::endl;
			}

			/**Returns points from the MMF populated in the UpdateDD function*/			
			void GetNeighbours(size_t p_fromPoint, std::vector<size_t> &p_neighbours, size_t * r_addr) {				
				p_neighbours.clear(); //Clear the vector				
				size_t numberOfNeighbours = r_addr[p_fromPoint*(N_FOR_CRAWL+1)];
				//std::cout<< "T" << omp_get_thread_num() << " Point Idx = " << p_fromPoint << " #Neighbours= " << numberOfNeighbours << std::endl;
				for (int i = 1, j=p_fromPoint*(N_FOR_CRAWL+1)+1; i<=numberOfNeighbours; i++, j++) p_neighbours.push_back(r_addr[j]);
			}
			
			/**Returns points within a "reasonable" distance from p_fromPoint located in this and surrounding voxels of the sparse grid*/			
			void GetNeighbours(Point * p_fromPoint, std::vector<std::vector<Point*>*> &p_cellValues, std::vector<Point*> &p_neighbours) {				
				p_neighbours.clear(); //Clear the vector
				for (std::vector<std::vector<Point*>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<Point*>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {
						if (Vector3::DistanceSquared((*pointIterator)->position, p_fromPoint->position) < m_neighbourRadiusSq) { 
							p_neighbours.push_back(*pointIterator);
						}	
					}
				}
			}						
			
			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			void GetNeighbours(size_t p_fromPoint, std::vector<std::vector<size_t>*> &p_cellValues, std::vector<Point*> &p_neighbours, int p_numberOfNeighbours) {				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				int distIdx = 0;
				float crtDist = 0.0f;				
				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(m_pointCloud->GetPointAt(p_fromPoint)->position.Element);
				for (std::vector<std::vector<size_t>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {												
						a = _mm_load_ps(m_pointCloud->GetPointAt(*pointIterator)->position.Element);												
						a = _mm_sub_ps(a, b);
						res = _mm_dp_ps(a, a, mask);
						crtDist = res.m128_f32[3];						
						if (crtDist < distances[p_numberOfNeighbours-1]) {
							for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
								if (crtDist <= distances[distIdx]) {								
									distances.insert(distances.begin()+distIdx, crtDist);									
									p_neighbours.insert(p_neighbours.begin()+distIdx, m_pointCloud->GetPointAt(*pointIterator));									
									break;
								}								
							}
						}						
					}
				}				
			}

			/**Returns the closest p_numberOfNeighbours from p_fromPoint located in the surrounding voxels of the sparse grid*/
			void GetNeighbours(size_t p_fromPoint, std::vector<std::vector<size_t>*> &p_cellValues, std::vector<size_t> &p_neighbours, int p_numberOfNeighbours) {				
				p_neighbours.clear(); //Clear the list
				std::vector<float> distances = std::vector<float>(p_numberOfNeighbours, 100); 				
				int distIdx = 0;
				float crtDist = 0.0f;				
				__m128 a,b;
				__m128 res;
				const int mask = 0xF8;				
				b = _mm_load_ps(m_pointCloud->GetPointAt(p_fromPoint)->position.Element);
				for (std::vector<std::vector<size_t>*>::iterator cellIterator = p_cellValues.begin(); cellIterator != p_cellValues.end(); ++cellIterator) {
					for (std::vector<size_t>::iterator pointIterator = (*cellIterator)->begin(); pointIterator != (*cellIterator)->end(); ++pointIterator) {												
						a = _mm_load_ps(m_pointCloud->GetPointAt(*pointIterator)->position.Element);												
						a = _mm_sub_ps(a, b);
						res = _mm_dp_ps(a, a, mask);
						crtDist = res.m128_f32[3];						
						if (crtDist < distances[p_numberOfNeighbours-1]) {
							for (distIdx = 0; distIdx < p_numberOfNeighbours; ++distIdx) {
								if (crtDist <= distances[distIdx]) {								
									distances.insert(distances.begin()+distIdx, crtDist);									
									p_neighbours.insert(p_neighbours.begin()+distIdx, *pointIterator);									
									break;
								}								
							}
						}						
					}
				}				
			}

			/**Calculates the covariance matrix of the first p_numberOfNeighbours points in p_from. Stores it in p_result*/
			void CalcCovMatrix(std::vector<size_t> &p_from, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<size_t>::iterator start = p_from.begin();
				std::vector<size_t>::iterator end;

				if (p_from.size() > p_numberOfNeighbours) end = p_from.begin()+p_numberOfNeighbours;
				else end = p_from.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
								
				for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (m_pointCloud->GetPointAt(*pointIterator))->position.X;
					median.Y += (m_pointCloud->GetPointAt(*pointIterator))->position.Y;
					median.Z += (m_pointCloud->GetPointAt(*pointIterator))->position.Z;
				}
				median /= p_numberOfNeighbours;

				//Now calculate diagonal of cov matrix
				for (std::vector<size_t>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X);
					c11 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y);
					c22 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
					c01 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y);
					c02 += ((m_pointCloud->GetPointAt(*pointIterator))->position.X - median.X) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
					c12 += ((m_pointCloud->GetPointAt(*pointIterator))->position.Y - median.Y) * ((m_pointCloud->GetPointAt(*pointIterator))->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

			void CalcCovMatrix(std::vector<Point*> &p_from, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<Point*>::iterator start = p_from.begin();
				std::vector<Point*>::iterator end;

				if (p_from.size() > p_numberOfNeighbours) end = p_from.begin()+p_numberOfNeighbours;
				else end = p_from.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
								
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (*pointIterator)->position.X;
					median.Y += (*pointIterator)->position.Y;
					median.Z += (*pointIterator)->position.Z;
				}
				median /= p_numberOfNeighbours;

				//Now calculate diagonal of cov matrix
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.X - median.X);
					c11 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Y - median.Y);
					c22 += ((*pointIterator)->position.Z - median.Z) * ((*pointIterator)->position.Z - median.Z);
					c01 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Y - median.Y);
					c02 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Z - median.Z);
					c12 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

			void CalcCovMatrix(std::vector<Point*> &p_from0, std::vector<Point*> &p_from1, std::vector<Point*> &p_from2, int p_numberOfNeighbours, Eigen::Matrix3f &p_result)
			{				
				float c00, c11, c22, c01, c02, c12;
				c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;
				std::vector<Point*>::iterator pointIterator;
				std::vector<Point*>::iterator start = p_from1.begin();
				std::vector<Point*>::iterator end;

				if (p_from1.size() > p_numberOfNeighbours) end = p_from1.begin()+p_numberOfNeighbours;
				else end = p_from1.end();
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					p_from0.push_back(*pointIterator);
				}

				start = p_from2.begin();
				if (p_from2.size() > p_numberOfNeighbours) end = p_from2.begin()+p_numberOfNeighbours;
				else end = p_from2.end();
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					p_from0.push_back(*pointIterator);
				}
							
				start = p_from0.begin();
				if (p_from0.size() > p_numberOfNeighbours*3) end = p_from0.begin()+(p_numberOfNeighbours*3);
				else end = p_from0.end();

				//First Get Median Vector
				Vector3 median = Vector3(0);
				int pointCount = 0;
								
				for (pointIterator = start; pointIterator != end; ++pointIterator) {
					median.X += (*pointIterator)->position.X;
					median.Y += (*pointIterator)->position.Y;
					median.Z += (*pointIterator)->position.Z;
					pointCount++;
				}

				median /= pointCount;

				//Now calculate diagonal of cov matrix
				for (std::vector<Point*>::iterator pointIterator = start; pointIterator != end; ++pointIterator) {
					c00 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.X - median.X);
					c11 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Y - median.Y);
					c22 += ((*pointIterator)->position.Z - median.Z) * ((*pointIterator)->position.Z - median.Z);
					c01 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Y - median.Y);
					c02 += ((*pointIterator)->position.X - median.X) * ((*pointIterator)->position.Z - median.Z);
					c12 += ((*pointIterator)->position.Y - median.Y) * ((*pointIterator)->position.Z - median.Z);
				}				

				p_result << c00 / p_numberOfNeighbours, c01 / p_numberOfNeighbours, c02 / p_numberOfNeighbours, 
							c01 / p_numberOfNeighbours, c11 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, 
							c02 / p_numberOfNeighbours, c12 / p_numberOfNeighbours, c22 / p_numberOfNeighbours;
								
			}

		};		

	}
}
									